﻿/*
 * Copyright © 2020, Wuxi Safover Intelligent Technology Co., Ltd. All rights reserved.
 * 
 * 无锡安永智能科技有限公司版权所有，未经许可，不得用于商业用途
 * 
 * Visit: https://www.safover.com/ 
 */

using log4net;
using System;
using System.Reflection;
using Safover.LidarCollector.Models;

namespace Safover.LidarCollector.Managers
{
    public class VerticalScanControlLogic
    {
        private static readonly ILog Log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);

        private static string Name => "VerticalScanControlLogic";

        public const int PIAngle = 360;
        public const int CircularConstant = 180;
        public const int InverseThreshold = 90;

        public float StartAngle { get; }

        public float StepAngle { get; }

        /// <summary>
        /// 需扫描的角行程量
        /// </summary>
        public float RangeAngle { get; }


        public float EndAngle => (StartAngle + RangeAngle) % CircularConstant;

        public float CurrentAngle { get; private set; }

        public bool HasInversed { get; private set; }

        public bool ShouldMoveOpposite { get; private set; }

        public float FixedHorizontalAngle { get; private set; }

        public VerticalBlindSectorConfig[] BlindSectors { get; set; }

        public bool AbleToScan
        {
            get
            {
                float scanedAngleRange;

                if (HasInversed)
                {
                    scanedAngleRange = InverseThreshold - StartAngle + InverseThreshold - CurrentAngle;
                }
                else 
                {
                    scanedAngleRange = CurrentAngle - StartAngle;
                }
                
                return scanedAngleRange <= RangeAngle;
            }
        }

        /// <summary>
        /// startAngle + rangeAngle:  (0, 180]
        /// </summary>
        /// <param name="startAngle">startAngle: [0, 90)</param>
        /// <param name="rangeAngle">rangeAngle: (0, 180]</param>
        /// <param name="stepAngle">stepAngle: (0, 180]</param>
        /// <param name="fixedHorizontalAngle">fixedHorizontalAngle: [0, 359)</param>
        public VerticalScanControlLogic(float startAngle, float rangeAngle, float stepAngle, float fixedHorizontalAngle, VerticalBlindSectorConfig[] blindSectors = null)
        {
            Check(startAngle, rangeAngle, stepAngle, fixedHorizontalAngle);

            StartAngle = startAngle;
            RangeAngle = rangeAngle;
            StepAngle = stepAngle;
            FixedHorizontalAngle = fixedHorizontalAngle;
            CurrentAngle = StartAngle;

            BlindSectors = blindSectors;
        }

        private void Check(float startAngle, float rangeAngle, float stepAngle, float fixedHorizontalAngle) 
        {
            // 起始角度：[0-90)
            if (startAngle < 0 || startAngle >= InverseThreshold)
            {
                throw new ArgumentOutOfRangeException($"{Name}: startAngle=[{startAngle}] is out of the range=[0 - {InverseThreshold})");
            }

            // 行程角度量：(0-180]
            if (rangeAngle <= 0 || rangeAngle > CircularConstant)
            {
                throw new ArgumentOutOfRangeException($"{Name}: rangeAngle=[{rangeAngle}] is out of the range=(0 - {CircularConstant}]");
            }

            // 步长角度必须大于0 且小于等于 180
            if (stepAngle <= 0 || stepAngle > 180)
            {
                throw new ArgumentOutOfRangeException($"{Name}: stepAngle=[{stepAngle}] should be within (0-180]");
            }

            if (fixedHorizontalAngle < 0 || fixedHorizontalAngle >= PIAngle)
            {
                throw new ArgumentOutOfRangeException($"{Name}: fixedHorizontalAngle=[{fixedHorizontalAngle}] is out of the range=[0 - {PIAngle})");
            }

            // 起始角 + 行程量 <= 180
            if (startAngle + rangeAngle > CircularConstant)
            {
                throw new ArgumentOutOfRangeException($"{Name}: sum of startAngle=[{startAngle}] + rangeAngle=[{rangeAngle}] = [{startAngle + rangeAngle}] is out of the range of scanable threshold=[{CircularConstant}]");
            }
        }

        public VScanInstruction Tick()
        {
            var instruction = DoTick();

            return instruction;
        }

        private VScanInstruction DoTick()
        {
            if (!AbleToScan)
            {
                return new VScanInstruction(false, null, false, FixedHorizontalAngle);
            }

            var instruction = new VScanInstruction(true, CurrentAngle % CircularConstant, ShouldMoveOpposite, FixedHorizontalAngle);

            var virtualNextAngle = CurrentAngle + StepAngle;

            ShouldMoveOpposite = false;
            if (!HasInversed && virtualNextAngle > InverseThreshold)
            {
                HasInversed = true;
                // ShouldMoveOpposite在一个指令周期内最多只会被反转一次
                ShouldMoveOpposite = true;
                FixedHorizontalAngle = (FixedHorizontalAngle + CircularConstant) % PIAngle;
                CurrentAngle = InverseThreshold - (CurrentAngle + StepAngle) % InverseThreshold;
            }
            else if (HasInversed)
            {
                CurrentAngle = (CurrentAngle - StepAngle) % InverseThreshold;
            }
            else
            {
                CurrentAngle += StepAngle;
            }

            return WithinBlind(instruction) ? DoTick() : instruction;
        }

        private bool WithinBlind(VScanInstruction instruction)
        {
            var blindSectors = BlindSectors;

            if (blindSectors == null || blindSectors.Length <= 0)
            {
                return false;
            }

            foreach (VerticalBlindSectorConfig blindSector in blindSectors)
            {
                if (blindSector.IsInside((float)instruction.ScanAngle))
                {
                    Log.DebugWriteLine($"{Name} scan angle=[{instruction.ScanAngle}] IS within blind sector=[{blindSector}].");
                    return true;
                }

                Log.DebugWriteLine($"{Name} scan angle=[{instruction.ScanAngle}] IS NOT within blind sector=[{blindSector}].");
            }

            return false;
        }

        public override string ToString()
        {
            return $"StartAngle={StartAngle}, RangeAngle={RangeAngle}, StepAngle={StepAngle}, CurrentAngle={CurrentAngle}, AbleToScan={AbleToScan}, HasInversed={HasInversed}.";
        }
    }

}
